/*
 * joy_platform.cpp
 *
 *  Created on: Dec 4, 2012
 *      Author: michiel
 */

#include <geometry_msgs/Twist.h>

#include "navigation/joy_platform.h"

#define CLIP(x, l, u) ((x)<(l))?(l):((x)>(u))?(u):(x)

void JoyPlatform::init()
{
  // Read limits
  ROS_ASSERT(nh_.getParam("forward_max", forward_max_));
  ROS_ASSERT(nh_.getParam("turn_max", turn_max_));

  joy_sub_ = nh_.subscribe(Topic::PS3_JOY, 10, &JoyPlatform::joyCallback, this);
  vel_pub_ = nh_.advertise<geometry_msgs::Twist>(Topic::NAV_VELOCITY, 10);
}

void JoyPlatform::spin()
{
  // Publish velocity at 20 Hz
  ros::Rate loop_rate(20);

  while (ros::ok())
  {
		ros::spinOnce();

		// Clip velocities to limits
		geometry_msgs::Twist twist;
		twist.linear.x = CLIP(forward_vel_, -forward_max_, forward_max_);
		twist.angular.z = CLIP(turn_rate_, -turn_max_, turn_max_);
		vel_pub_.publish(twist);

		loop_rate.sleep();
  }
}

void JoyPlatform::joyCallback(const sensor_msgs::Joy::ConstPtr &msg)
{
  forward_vel_ = msg->axes[PS3_AXIS_STICK_LEFT_UPWARDS]*forward_max_;
  turn_rate_ = msg->axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS]*turn_max_;
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "joy_platform_node");

	JoyPlatform controller;
	controller.spin();
}

